/*
 * Scan_distance.c
 *
 * Created: 11/20/2013 3:18:17 PM
 *  Author: Daniel Keith
 */ 


#include <avr/io.h>
#include <avr/io.h>
#include <avr/interrupt.h>
#include "util.h"
#include <stdio.h>
#include <string.h>
#include <math.h>
#include "servo.h"
#include "IR_sensor.h"
#include "Sonar_sensor.h"
#include "BAM.h"

int offset = 62; //the offset for the servo //62 for robot #16 // 38 for robot #8

/** scans 180 degrees
*	does a 180 scan and detects all object 60cm out and prints to terminal the distance, radius, and degree of the objects.
*   all objects with a radius greater than 0 and with a valid distance have there data transmitted to the terminal.
*/

void scan(void)
{
	serial_init();
	init_ser();
	int degree =0;					//keeps track of the servo degree
	int s_degree =0, f_degree =0;	//records the first degree and last degree of an object
	int distance_IR =0;				//IR distance readings	
	int dist =0;					//The distance of the object when it is first detected
	int state =0;					//enabled when an object is detected
	int control =0;					//controls when the radius of an object is calculated
	int radius =0;					//The radius of the object
	int print =0;					//controls when the header is printed
	
	//prevents error in the sensor cause by servo backlash
	move_servo(degree+offset);
	wait_ms(500);
	
    while(degree <= 180)
    {
		
		//moves the servo two degrees at a time and keeps the servo in the correct range
		
			move_servo(degree+offset);
			degree = degree + 2;
			wait_ms(100);
		
		//gets the IR  distance values
		distance_IR = IR_cal();

		
		 // checks to see if an object is detected in the wanted range for the IR sensor		
		if(((distance_IR >= 9) && (distance_IR < 60)) )
		{
			if (state ==0)
			{
				s_degree = degree;//saves the degree position of the beginning edge of the object
				dist = distance_IR;
			}
			
			state =1;
			control =0;
			
		}
		else
		{
			state =0;
			control++;
			
			//uses the variable "control" to stop calculations from happening constantly
			if(control ==1)
			{
				
				f_degree = degree;//saves the degree position of the far edge of the object
			
			
				 radius = (f_degree-s_degree)/2; // measures the width of the object
				//int width = tan(radius)*(2.0*distance_object); // needs to be made more accurate before use.
					
				//checks to see if the radius is valid then prints to terminal the distance, radius, and degree of the objects
				if (radius != 0 && dist !=0)
				{
						//prints the distance, radius, and degree of the objects headers
						if (print ==0)
						{
							
							serial_puts("Degree\t radius\t distance\r\n");
							print = 1;
						}
						
						//prints the distance, radius, and degree of the objects
						
							//prints out the degree of the servo to the terminal
							char buf[10];
							sprintf(buf, "%d\t",degree);
							serial_puts(buf);
					
							//prints out the radius of the objects found by the IR sensor to the terminal
							char buf1[10];
							sprintf(buf1, "%d\t", radius);
							serial_puts(buf1);
					
							//prints out the IR distance to the terminal
							char buf2[10];
							sprintf(buf2, "%d\t\r\n", dist);
							serial_puts(buf2);
				}
			}				
		}
		
	 }
	  //prints to terminal that the scan is done
      serial_puts("Done scanning \r\n");  		
}

/** scans the front
*	moves to 45 degrees and scans for objects that are less than 50 cm in front of the robot
*	transmits to terminal the distance and degree of any objects in front of it
*	returns 0 if it finds nothing is found 
*/ 

void scan_front(void){

	
	init_ser();
	int print =0;			//enable the print of the headers
	int IR_distance =0;		//the IR distance readings
	int start_degree = 45;	//keeps track of the current degree of the servo
	int state1 =0;			//is one when an object is being scanned
	int rad =0;				//The radius of the object
	int dist1 =0;			//The distance of the object when it is first detected
	int control1 =0;		//controls when the radius is calculated
	int s_degree1 =0;		//records the first degree of the object
	int f_degree1 =0;		//records the last degree of the object
	
	//prevents error in the sensor cause by servo backlash
	move_servo(start_degree+offset);
	wait_ms(500);
	
	//runs till the front of the robot has be scanned
	while (start_degree < 135)
	{
	
		move_servo(start_degree + offset);
		start_degree = start_degree + 2;
		
		IR_distance = IR_cal();
		wait_ms(100);
		
		 // checks to see if an object is detected in the wanted range for the IR sensor
		 if(((IR_distance >= 9) && (IR_distance < 60)) )
		 {
			 if (state1 ==0)
			 {
				 s_degree1 = start_degree;//saves the degree position of the beginning edge of the object
				 dist1 = IR_distance;
			 }
			 
			 state1 =1;
			 control1 =0;
			 
		 }
		 else
		 {
			 
			 state1 =0;
			 control1++;
			 
			 //uses the variable "control" to stop calculations from happening constantly
			 if(control1 ==1)
			 {
				
				 f_degree1 = start_degree;//saves the degree position of the far edge of the object
				 
				 
				 rad = (f_degree1-s_degree1)/2; // measures the width of the object
				 
			//check to see if any object are 50 cm in front of the robot
			if (rad != 0 && dist1 != 0)
			{
				//prints the distance and degree of the objects headers
				if (print ==0)
				{
					
					serial_puts("\r\nDegree\t radius\t distance\r\n");
					print =1;
					
				}
				
				
				//prints the distance, radius, and degree of the objects
			
					//prints out the degree of the servo to the terminal
					char buf[10];
					sprintf(buf, "%d\t", start_degree);
					serial_puts(buf);
	
					//prints out the radius to the terminal
					char buf3[10];
					sprintf(buf3, "%d\t", rad);
					serial_puts(buf3);
					
					//prints out the IR distance to the terminal
					char buf2[10];
					sprintf(buf2, "%d\r\n", dist1);
					serial_puts(buf2);
					
					
					
					
			}
	
		}
	}
	}	
	
	//prints to terminal that the scan is done	 	
	serial_puts("Done scanning \r\n");
}
